#include <Inventor/actions/SoSearchAction.h>
#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/nodes/SoIndexedFaceSet.h>
#include <Inventor/nodes/SoCoordinate3.h>

#include <assert.h>
#include <iostream>
#include <list>
#include <vector>
#include <math.h>

#include "halfedges.h"
#include "vec3f.h"

HE_edge::HE_edge(int _vert, HE_face *_face) : vert(_vert), pair(NULL), face(_face), next(NULL), prev(NULL), mark(false) {
}

HE_edge::HE_edge() : vert(-1),/* orig(-1),*/ pair(NULL), face(NULL), next(NULL), prev(NULL), mark(false) {
}

HE_face::HE_face() : edge(NULL), size(0), dart(false), tip(NULL) {
}

Mesh::Mesh() : edges(), faces(), verts() {
}

Mesh::~Mesh() {
	for (vector<HE_face *>::iterator p=faces.begin(); p != faces.end(); p++) {
		delete (HE_face *)(*p);
	}
	for (vector<HE_edge *>::iterator p=edges.begin(); p != edges.end(); p++) {
		delete (HE_edge *)(*p);
	}
}

void Mesh::clear_edge_marks() {
	for (vector<HE_edge *>::iterator p=edges.begin(); p != edges.end(); p++) {
		HE_edge *edge = *p;
		edge->mark = false;
	}
}

bool Mesh::doo_sabin(vector<vector<float> > coeffs) {
	clear_edge_marks();
	vector<int> idxverts2(faces.size(), -1);
	for (vector<HE_face *>::iterator p=faces.begin(); p!=faces.end(); p++) {
		HE_face *face = *p;
		assert(face && face->tip && face->edge && 4==face->size);
		if (face->dart) continue; // will create new vertices for kites only
		float point[3] = {0,0,0};
		for (int k=0; k<3; k++) {
//			float coeffs[] = {.7, .125, .05, .125};
			HE_edge *tmp = face->tip;
			for (int z=0;z<4;z++) {
				point[k] += coeffs[1][z]*verts[tmp->vert*3+k];
				tmp = tmp->next;
			}
		}
		idxverts2[p-faces.begin()] = verts.size()/3;
		for (int k=0; k<3; k++) {
			verts.push_back(point[k]);
		}
	}
	vector<int> idxverts3(faces.size(), -1);
	for (vector<HE_face *>::iterator p=faces.begin(); p!=faces.end(); p++) {
		HE_face *f1 = *p;
		assert(f1 && f1->tip && f1->edge && 4==f1->size);
		HE_edge *edge = (f1->dart?f1->tip->next:f1->tip);
		if (!edge->pair) continue; // new vertices for interior edges only
		HE_face *f2 = edge->pair->face;
		float point[3] = {0,0,0};
		if (f1->dart && f2->dart) {
			for (int k=0; k<3; k++) {
				point[k] = coeffs[0][0]*verts[edge->prev->vert*3+k] +
				           coeffs[0][1]*verts[edge->vert*3+k] +
				           coeffs[0][2]*verts[edge->next->vert*3+k] +
				           coeffs[0][4]*verts[edge->next->next->vert*3+k] +
				           coeffs[0][5]*verts[edge->pair->next->vert*3+k] +
				           coeffs[0][3]*verts[edge->pair->next->next->vert*3+k];
			}
		} else if (!f1->dart && !f2->dart) {
			for (int k=0; k<3; k++) {
				point[k] = coeffs[2][0]*verts[edge->vert*3+k] +
				           coeffs[2][1]*verts[edge->prev->vert*3+k] +
				           coeffs[2][5]*verts[edge->next->vert*3+k] +
				           coeffs[2][3]*verts[edge->next->next->vert*3+k] +
				           coeffs[2][2]*verts[edge->pair->next->vert*3+k] +
				           coeffs[2][4]*verts[edge->pair->next->next->vert*3+k];
			}
		} else {
			if (!f1->dart) {
				for (int k=0; k<3; k++) {
					point[k] = coeffs[4][0]*verts[edge->vert*3+k] +
					           coeffs[4][3]*verts[edge->prev->vert*3+k] +
					           coeffs[4][5]*verts[edge->next->vert*3+k] +
					           coeffs[4][4]*verts[edge->next->next->vert*3+k] +
					           coeffs[4][2]*verts[edge->pair->next->vert*3+k] +
					           coeffs[4][1]*verts[edge->pair->next->next->vert*3+k];
				}
			} else {/*
				float coeffs[] = {.15, .1, .15, .1, .125, .125, .125, .125};
				for (int k=0; k<3; k++) {
					HE_edge *l = edge;
					HE_edge *r = edge->pair->face->tip;
					for (int cnt=0; cnt<4; cnt++) {
						point[k] += coeffs[cnt*2]*verts[l->vert*3+k] + coeffs[cnt*2+1]*verts[r->vert*3+k];
						l = f1->dart ? l->next : l->prev;
						r = f2->dart ? r->prev : r->next;
					}
				}
				*/
				for (int k=0; k<3; k++) {
					point[k] = coeffs[4][0]*verts[edge->vert*3+k] +
					           coeffs[4][3]*verts[edge->prev->vert*3+k] +
					           coeffs[4][1]*verts[edge->next->vert*3+k] +
					           coeffs[4][2]*verts[edge->next->next->vert*3+k] +
					           coeffs[4][4]*verts[edge->pair->next->vert*3+k] +
					           coeffs[4][5]*verts[edge->pair->next->next->vert*3+k];
				}
			}
		}
		idxverts3[p-faces.begin()] = verts.size()/3;
		for (int k=0; k<3; k++) {
			verts.push_back(point[k]);
		}
	}


	vector<HE_edge *> newedges;
	vector<HE_face *> newfaces;

	for (vector<HE_face *>::iterator p=faces.begin(); p!=faces.end(); p++) {
		HE_face *face = *p;
		HE_edge *edge = (face->dart?face->tip->next:face->tip);
		if (!face->dart) {
			if (edge->pair) {
				{
				//kite
				HE_face *k1 = new HE_face();
				HE_edge *newe[] = {NULL, NULL, NULL, NULL};
				newe[0] = new HE_edge(edge->prev->vert, k1);
				newe[1] = new HE_edge(idxverts3[p-faces.begin()], k1);
				newe[2] = new HE_edge(idxverts2[p-faces.begin()], k1);
				newe[3] = new HE_edge(edge->next->next->vert, k1);
				for (int z=0; z<4; z++) {
					newe[z]->next = newe[(z+1)%4];
					newe[z]->prev = newe[(z-1+4)%4];
					newedges.push_back(newe[z]);
				}
				k1->tip  = newe[0];
				k1->edge = newe[0];
				k1->size = 4;
				k1->dart = false;
				newfaces.push_back(k1);
				}
				{

				//dart
				HE_face *d = new HE_face();
				HE_edge *newe[] = {NULL, NULL, NULL, NULL};
				newe[0] = new HE_edge(edge->vert, d);
				newe[1] = new HE_edge(idxverts2[p-faces.begin()], d);
				newe[2] = new HE_edge(idxverts3[p-faces.begin()], d);
				if (!edge->pair->face->dart) {
				int z = -1;
				for (vector<HE_face *>::iterator p2=faces.begin(); p2!=faces.end(); p2++) {
					if (*p2==edge->pair->face) {z = p2-faces.begin(); break;}
				}
				assert(z>=0);
				newe[3] = new HE_edge(idxverts2[z], d);
				} else {
					newe[3] = new HE_edge(edge->pair->next->next->vert, d);
				}
				for (int z=0; z<4; z++) {
					newe[z]->next = newe[(z+1)%4];
					newe[z]->prev = newe[(z-1+4)%4];
					newedges.push_back(newe[z]);
				}
				d->tip  = newe[0];
				d->edge = newe[0];
				d->size = 4;
				d->dart = true;
				newfaces.push_back(d);
				}

			}
			if (edge->next->pair) {
				//kite
				HE_face *k2 = new HE_face();
				HE_edge *newe[] = {NULL, NULL, NULL, NULL};
				newe[0] = new HE_edge(edge->next->vert, k2);
				newe[1] = new HE_edge(edge->next->next->vert, k2);
				newe[2] = new HE_edge(idxverts2[p-faces.begin()], k2);
				int z = -1;
				for (vector<HE_face *>::iterator p2=faces.begin(); p2!=faces.end(); p2++) {
					if (*p2==edge->next->pair->face) {z = p2-faces.begin(); break;}
				}
				assert(z>=0);
				newe[3] = new HE_edge(idxverts3[z], k2);
				for (int z=0; z<4; z++) {
					newe[z]->next = newe[(z+1)%4];
					newe[z]->prev = newe[(z-1+4)%4];
					newedges.push_back(newe[z]);
				}
				k2->tip  = newe[0];
				k2->edge = newe[0];
				k2->size = 4;
				k2->dart = false;
				newfaces.push_back(k2);
			}
		} else {
			if (edge->pair) {
				{
				//dart
				HE_face *d = new HE_face();
				HE_edge *newe[] = {NULL, NULL, NULL, NULL};
				newe[0] = new HE_edge(edge->vert, d);
				newe[1] = new HE_edge(edge->next->vert, d);
				newe[2] = new HE_edge(idxverts3[p-faces.begin()], d);
				if (!edge->pair->face->dart) {
					int z = -1;
					for (vector<HE_face *>::iterator p2=faces.begin(); p2!=faces.end(); p2++) {
						if (*p2==edge->pair->face) {z = p2-faces.begin(); break;}
					}
					assert(z>=0);
					newe[3] = new HE_edge(idxverts2[z], d);
				} else {
					newe[3] = new HE_edge(edge->pair->next->next->vert, d);
				}
				for (int z=0; z<4; z++) {
					newe[z]->next = newe[(z+1)%4];
					newe[z]->prev = newe[(z-1+4)%4];
					newedges.push_back(newe[z]);
				}
				d->tip  = newe[0];
				d->edge = newe[0];
				d->size = 4;
				d->dart = true;
				newfaces.push_back(d);
				}
				if (edge->prev->pair) {
					//kite
					HE_face *k = new HE_face();
					HE_edge *newe[] = {NULL, NULL, NULL, NULL};
					newe[0] = new HE_edge(edge->prev->vert, k);
					newe[1] = new HE_edge(idxverts3[p-faces.begin()], k);
					newe[2] = new HE_edge(edge->next->vert, k);
				int z = -1;
				for (vector<HE_face *>::iterator p2=faces.begin(); p2!=faces.end(); p2++) {
					if (*p2==edge->prev->pair->face) {z = p2-faces.begin(); break;}
				}
				assert(z>=0);
				newe[3] = new HE_edge(idxverts3[z], k);
					for (int z=0; z<4; z++) {
						newe[z]->next = newe[(z+1)%4];
						newe[z]->prev = newe[(z-1+4)%4];
						newedges.push_back(newe[z]);
					}
					k->tip  = newe[0];
					k->edge = newe[0];
					k->size = 4;
					k->dart = false;
					newfaces.push_back(k);
				}
			}

		}
	}
//	cerr << "<<<\n";
	vector<float> replverts;
	vector<int> irverts;
	vector<bool> vmark(verts.size(), false);
	float phi = (sqrt(5)+1.)/2.0;
	for (vector<HE_face *>::iterator p=faces.begin(); p!=faces.end(); p++) {
		HE_face *face = *p;
		HE_edge *fedge = face->tip;
		do {
			if (!vmark[fedge->vert]) {
				HE_edge *edge = fedge;
//				bool allsame = true;
				bool interior = true;
				int cnt = 0;
				int ndarts = 0;
				int nkites = 0;
				int seq[] = {0,0,0,0,0,0};
				do {
//					if (edge->face->dart!=face->dart) {
//						allsame = false;
//					}
					if (edge->face->dart) {
						seq[cnt] = 1;
						ndarts++;
					} else {
						nkites++;
					}
					if (edge->pair) {
						edge = edge->pair->prev;
					} else {
						interior = false;
						vmark[fedge->vert] = true;
						break;
					}
					cnt++;
				} while (edge!=fedge);
				if (!interior) continue;
				edge = fedge;
				float point[3] = {0,0,0};
				if (5==nkites) { //sun
					float l0 = coeffs[5][0];
					float l1 = coeffs[5][1];
					do {
						for (int i=0; i<3; i++) {
							if (edge==fedge) {
								point[i] += l0*verts[edge->vert*3+i];
							}
							point[i] += l1*verts[edge->prev->vert*3+i];
							point[i] += l1*verts[edge->prev->prev->vert*3+i];
						}
						edge = edge->pair->prev;
					} while (edge!=fedge);
					cerr << "sun\n";
				} else if (5==ndarts) { //star
					float l0 = coeffs[3][0];
					float l1 = coeffs[3][1];
					float l2 = coeffs[3][2];
					do {
						for (int i=0; i<3; i++) {
							if (edge==fedge) {
								point[i] += l0*verts[edge->vert*3+i];
							}
							point[i] += l1*verts[edge->prev->vert*3+i];
							point[i] += l2*verts[edge->prev->prev->vert*3+i];
						}
						edge = edge->pair->prev;
					} while (edge!=fedge);
					cerr << "star\n";
				} else if (1==ndarts && 2==nkites) {
					do {
						if (edge->face->dart) break;
						edge = edge->pair->prev;
					} while (edge!=fedge);
					float l0 = .75;
					float l1 = .05;
					float l2 = .05;
					float l3 = (-1 + 1*l0 + l1*(1+1/phi)+l2*(-1/(phi*phi)+2))/(1-phi);
					float l4 = 1 - l0 - l1 - 2*l2 - 2*l3;
					for (int i=0; i<3; i++) {
						point[i] += l0*verts[edge->vert*3+i];
						point[i] += l1*verts[edge->prev->prev->vert*3+i];
						point[i] += l2*verts[edge->prev->vert*3+i];
						point[i] += l2*verts[edge->next->vert*3+i];
						point[i] += l3*verts[edge->pair->next->vert*3+i];
						point[i] += l3*verts[edge->next->pair->prev->prev->vert*3+i];
						point[i] += l4*verts[edge->next->pair->next->vert*3+i];
					}
//					cerr << l0 << " " << l1 << " " << l2 << " " << l3 << " " << l4 << " "  << endl;
					cerr << "ace\n";
				} else if (2==ndarts && 2==nkites) { // deuce
					do {
						if (edge->face->dart && edge->pair->face->dart) break;
						edge = edge->pair->prev;
					} while (edge!=fedge);
					float l0 = .75;
					float l1 = .05;
					float l2 = .05;
					float l3 = .05;
					float l4 = (1 - l0 - l1*(1+phi) - l2*(phi*(3-phi)+2) - l3*(2+phi))/(2+phi*(1-phi));
					float l5 = 1 - l0 - l1 - 2*l2 - 2*l3 - 2*l4;

//					cerr << l0 << " " << l1 << " " << l2 << " " << l3 << " " << l4 << " " << l5 << " " << endl;
					for (int i=0; i<3; i++) {
						point[i] += l0*verts[edge->vert*3+i];
						point[i] += l1*verts[edge->prev->vert*3+i];
						point[i] += l2*verts[edge->next->next->vert*3+i];
						point[i] += l2*verts[edge->pair->next->vert*3+i];
						point[i] += l3*verts[edge->next->vert*3+i];
						point[i] += l3*verts[edge->pair->prev->prev->vert*3+i];
						point[i] += l4*verts[edge->next->pair->next->pair->prev->prev->vert*3+i];
						point[i] += l4*verts[edge->next->pair->next->next->vert*3+i];
						point[i] += l5*verts[edge->next->pair->next->vert*3+i];
					}
					cerr << "deuce\n";
				} else {
					float l0 = coeffs[5][0];
					do {
						for (int i=0; i<3; i++) {
							if (edge==fedge) {
								point[i] += l0*verts[edge->vert*3+i];
							}
							point[i] += (1-l0)/(2.0*cnt)*verts[edge->prev->vert*3+i];
							point[i] += (1-l0)/(2.0*cnt)*verts[edge->prev->prev->vert*3+i];
						}
						edge = edge->pair->prev;
					} while (edge!=fedge);
				}
				irverts.push_back(fedge->vert);
				for (int i=0; i<3; i++) {
					replverts.push_back(point[i]);
				}
				vmark[fedge->vert] = true;
			}
			fedge = fedge->next;
		} while (fedge!=face->tip);

/*		HE_face *face = *p;
		HE_edge *edge = face->tip;
		if (vmark[edge->vert]) continue;
		bool allsame = true;
		bool interior = true;
		int cnt = 0;
		do {
			if (edge->face->dart!=face->dart && edge->pair) {
				allsame = false;
			}
			if (edge->pair) {
				edge = edge->pair->prev;
			} else {
				interior = false;
				break;
			}
			cnt++;
		} while (edge!=face->tip);
		if (!interior) continue;
		if (allsame) {
			float point[3] = {0,0,0};
			edge = face->tip;
			do {
				for (int i=0; i<3; i++) {
					if (face->dart) {
						if (edge==face->tip) {
							point[i] += coeffs[3][0]*verts[edge->vert*3+i];
						}
						point[i] += coeffs[3][1]*verts[edge->prev->vert*3+i];
						point[i] += coeffs[3][2]*verts[edge->prev->prev->vert*3+i];
					} else {
						if (edge==face->tip) {
							point[i] += coeffs[5][0]*verts[edge->vert*3+i];
						}
						point[i] += coeffs[5][1]*verts[edge->prev->vert*3+i];
						point[i] += coeffs[5][1]*verts[edge->prev->prev->vert*3+i];
					}
				}
				edge = edge->pair->prev;
			} while (edge!=face->tip);
			irverts.push_back(edge->vert);
			vmark[edge->vert];
			for (int i=0; i<3; i++) {
				replverts.push_back(point[i]);
			}
		} else {
		}
		*/
	}

	for (int i=0; i<(int)irverts.size(); i++) {
		for (int k=0; k<3; k++) {
//			verts[irverts[i]*3+k] = replverts[i*3+k];
		}
	}

	for (vector<HE_face *>::iterator p=faces.begin(); p != faces.end(); p++) {
		delete (HE_face *)(*p);
	}
	for (vector<HE_edge *>::iterator p=edges.begin(); p != edges.end(); p++) {
		delete (HE_edge *)(*p);
	}
	faces = newfaces;
	edges = newedges;

	vector<list<HE_edge *> > vert_edges(verts.size());
	for (vector<HE_face *>::iterator p=faces.begin(); p!=faces.end(); p++) {
		HE_face *face = *p;
		HE_edge *edge = face->edge;
		do {
			assert(edge->next);
			int a = edge->vert;
			int b = edge->next->vert;
			// search b's edge list for edge going to a
			// assign half edge as pair
			list<HE_edge *>::iterator p2 = vert_edges[b].begin();
			for (; p2 != vert_edges[b].end(); p2++) {
				HE_edge *edge2 = *p2;
				assert(edge2);
				// found edge?
				if (edge2->vert!=a) continue;
				// link edges
				edge->next->pair = edge2;
				edge2->pair = edge->next;
				// remove edge and stop
				vert_edges[b].erase(p2);
				break;
			}
			// if not found and not paired, add edge to the vert's list
			if (vert_edges[b].end()==p2 && !edge->next->pair) {
				vert_edges[a].push_back(edge->next);
			}
			edge = edge->next;
		} while (edge!=face->edge);
	}

/*


	for (vector<HE_edge *>::iterator p=edges.begin(); p!=edges.end(); p++) {
		HE_edge *edge = *p;
			if (!edge->pair) {
				for (vector<HE_edge *>::iterator p2=edges.begin(); p2!=edges.end(); p2++) {
					HE_edge *edge2 = *p2;
						if (edge->vert==edge2->vert && edge->prev->vert==edge2->next->vert) {
							assert(!edge2->next->pair);
							edge->pair = edge2->next;
							edge2->next->pair = edge;
							break;
					}
					edge2 = edge2->next;
			}
		}
	}
*/
	return true;
}

float angle(Vector3f v21, Vector3f v23) {
	return atan2(Vector3f(v21^v23).norm(),v21*v23)*180./M_PI;
}

float angle(float x1, float y1, float z1, float x2, float y2, float z2, float x3, float y3, float z3) {
	Vector3f v21(x1-x2, y1-y2, z1-z2); // Vector from B to A
	Vector3f v23(x3-x2, y3-y2, z3-z2); // Vector from B to C
	return atan2(Vector3f(v21^v23).norm(),v21*v23)*180./M_PI;
}



std::ostream& operator<<(std::ostream& s, Mesh& he) {
	s << "Faces: " << he.faces.size() << "\n";
	// loop over faces
	for (vector<HE_face *>::iterator p=he.faces.begin(); p!=he.faces.end(); p++) {
		// loop over edges
		HE_face *face = *p;
		HE_edge *edge = face->edge;
		s << "Face: " << face->edge << " " << (face->dart ? "dart" : "kite") << " with the tip idx " << face->tip->vert << "\n";
		bool dart = false;
		do {
			s << "Edge: " << edge << " "
				<< edge->vert << " ("
				<< he.verts[edge->vert*3  ] << ", "
				<< he.verts[edge->vert*3+1] << ", "
				<< he.verts[edge->vert*3+2] << ") "
				<< edge->face << " "
				<< edge->pair << " "
				<< "prev: " << edge->prev << " "
				<< "next: " << edge->next << "\n";
			edge = edge->next;
		} while (edge != face->edge);
		if (dart) s << "it was a dart"<<endl;
	}
	return s;
}

// Loads a single faceset into the data structure.
// The strategy is to loop over faces, inserting half edges
// for each edge, which are linked together, including a link to a new face.
// For convenience, this new face is itself stored in a member list.
// Additionally, the half edges are stored in lists
// at each vertex. Then these new half edges are looped over again
// to compare with the lists at each vertex to identify half-edge pairs.
// Identified pairs are then removed from the vertex's edge list.
// In this way each half edge is linked with it's next half edge,
// a face, a pair, and of course a single vertex.
bool loadShape(SoIndexedFaceSet *shape, SoCoordinate3 *coords, Mesh &he) {
	for (vector<HE_face *>::iterator p=he.faces.begin(); p != he.faces.end(); p++) {
		delete (HE_face *)(*p);
	}
	for (vector<HE_edge *>::iterator p=he.edges.begin(); p != he.edges.end(); p++) {
		delete (HE_edge *)(*p);
	}
	he.edges = vector<HE_edge *>();
	he.faces = vector<HE_face *>();
	if (!shape || !coords || !shape->isOfType(SoIndexedFaceSet::getClassTypeId()) || !coords->isOfType(SoCoordinate3::getClassTypeId())) return false;

    int num_verts = coords->point.getNum();
	he.verts = vector<float>(num_verts*3);
	for (int i=0; i<num_verts; i++) {
		for (int j=0; j<3; j++) {
			he.verts[i*3+j] = coords->point[i][j];
		}
	}

    int num_idx   = shape->coordIndex.getNum();
	vector<list<HE_edge *> > vert_edges(num_verts);

	for (int j=0; j<num_idx; j++) { // loop over polygons, here j++ means skip -1 index
		HE_face *face = new HE_face();
		he.faces.push_back(face);
		HE_edge *prev_edge=NULL;
		for (; shape->coordIndex[j]>=0; j++) { // loop over vertices in the current polygon
			HE_edge *edge = new HE_edge(shape->coordIndex[j], face);
			he.edges.push_back(edge);
			face->size++;
			if (!face->edge) {
				face->edge = edge;
			} else {
				assert(prev_edge);
				prev_edge->next = edge;
//				edge->orig = prev_edge->vert; // for debugging purposes
			}
			edge->prev = prev_edge;
			prev_edge  = edge;
			// will be rewritten each time until the last edge
//			face->edge->orig = edge->vert;
			edge->next = face->edge;
			face->edge->prev = edge;
		}
		if (!face->edge) continue;
		HE_edge *edge = face->edge;
		do {
			assert(edge->next);
			int a = edge->vert;
			int b = edge->next->vert;
			// search b's edge list for edge going to a
			// assign half edge as pair
			list<HE_edge *>::iterator p = vert_edges[b].begin();
			for (; p != vert_edges[b].end(); p++) {
				HE_edge *edge2 = *p;
				assert(edge2);
				// found edge?
				if (edge2->vert!=a) continue;
				// link edges
				edge->next->pair = edge2;
				edge2->pair = edge->next;
				// remove edge and stop
				vert_edges[b].erase(p);
				break;
			}
			// if not found and not paired, add edge to the vert's list
			if (vert_edges[b].end()==p && !edge->next->pair) {
				vert_edges[a].push_back(edge->next);
			}
			edge = edge->next;
		} while (edge!=face->edge);



		// TODO ugly detection of darts
		edge = face->edge;
		bool dart = false;
		float a = 0;
		do {
			float x1 = he.verts[edge->vert*3  ];
			float y1 = he.verts[edge->vert*3+1];
			float z1 = he.verts[edge->vert*3+2];
			float x2 = he.verts[edge->next->vert*3  ];
			float y2 = he.verts[edge->next->vert*3+1];
			float z2 = he.verts[edge->next->vert*3+2];
			float x0 = he.verts[edge->prev->vert*3  ];
			float y0 = he.verts[edge->prev->vert*3+1];
			float z0 = he.verts[edge->prev->vert*3+2];

			Vector3f v10(x0-x1, y0-y1, z0-z1); // Vector from B to A
			Vector3f v12(x2-x1, y2-y1, z2-z1); // Vector from B to C
			Vector3f n = v10^v12;
			float b = angle(v10, v12);
			if (b>a || !face->tip) {
				face->tip = edge;
				a = b;
			}

			float x1p = he.verts[edge->prev->vert*3  ];
			float y1p = he.verts[edge->prev->vert*3+1];
			float z1p = he.verts[edge->prev->vert*3+2];
			float x2p = he.verts[edge->prev->next->vert*3  ];
			float y2p = he.verts[edge->prev->next->vert*3+1];
			float z2p = he.verts[edge->prev->next->vert*3+2];
			float x0p = he.verts[edge->prev->prev->vert*3  ];
			float y0p = he.verts[edge->prev->prev->vert*3+1];
			float z0p = he.verts[edge->prev->prev->vert*3+2];
			Vector3f v10p(x0p-x1p, y0p-y1p, z0p-z1p); // Vector from B to A
			Vector3f v12p(x2p-x1p, y2p-y1p, z2p-z1p); // Vector from B to C
			Vector3f np = v10p^v12p;
			if (angle(np,n)>90) {
				dart = true;
			}
			edge = edge->next;
		} while (edge != face->edge);
		face->tip = face->tip->next->next;
		face->dart = dart;
	}
	/*
    // check for unfound half-edge's
    int unpairedEdges = 0;
    int firstTime = 1;
	for (int i=0; i<num_verts; i++) {
		unpairedEdges += vert_edges[i].size();
		if (unpairedEdges && firstTime){
			cerr << "Unpaired vertices: \n";
			firstTime = 0;
		}
		if (vert_edges[i].size()) {
			cerr << "Vert " << i << ": ";
			// loop over unapred edges
			for (list <HE_edge *>::iterator p=vert_edges[i].begin(); p!=vert_edges[i].end(); ++p) {
				HE_edge *edge = *p;
				cerr << edge->vert << " ";
			}
			cerr << "\n";
		}
	}

    // Euler's formula: F+V-E=2
    // E.g., a cube:    6+8-12=2
    cerr << he.faces.size() << " faces, " << num_verts << " verts, " << he.edges.size() << " half edges (" << he.edges.size()/2 << " edges).\n";
    cerr << "Euler characteristic = " << he.faces.size()+num_verts - he.edges.size()/2 << ".\n";
	cerr << unpairedEdges << " unpaired half-edges.\n";
	*/
//    cerr << "Done loading face set.\n";
	return true;
}

#define SEND_TRIANGLES_INSTEAD_OF_FULL_FACES 1
bool unloadShape(SoIndexedFaceSet *faceset, SoCoordinate3 *coords, Mesh &he) {
	if (!coords || !faceset) return false;
	coords->point.setNum(0);
	int j = 0;
	for (vector<float>::iterator p=he.verts.begin(); p!=he.verts.end(); p+=3) {
		coords->point.set1Value(j++, *p, *(p+1), *(p+2));
	}

	// read half edges into face set
	faceset->coordIndex.setNum(0);
	// loop over faces
	int i = 0;
	for (vector<HE_face *>::iterator p=he.faces.begin(); p!=he.faces.end(); p++) {
		// loop over edges
		HE_edge *edge = (*p)->edge;
#if SEND_TRIANGLES_INSTEAD_OF_FULL_FACES
		for (int j=0;j<(*p)->edge->face->size-2; j++) {
			HE_edge *cedge = edge;
			for (int k=0; k<3; k++) {
				faceset->coordIndex.set1Value(i++, cedge->vert);
				cedge = cedge->next;
			}
			faceset->coordIndex.set1Value(i++, -1);
			edge = edge->next->next;
		}
		int idx = p-he.faces.begin();
		faceset->materialIndex.set1Value(idx*2,   ((*p)->dart?0:1));
		faceset->materialIndex.set1Value(idx*2+1, ((*p)->dart?0:1));
#else
		do {
			faceset->coordIndex.set1Value(i++, edge->vert);
			edge = edge->next;
		} while (edge != (*p)->edge);
		faceset->coordIndex.set1Value(i++, -1);
		int idx = p-he.faces.begin();
		faceset->materialIndex.set1Value(idx,   ((*p)->dart?0:1));
#endif
	}
	cerr << "total curv: " << he.total_curvature() << endl;
	return true;
}

float Mesh::total_curvature() {
	vector<bool> vmark(verts.size(), false);
	float total_curv = 0;
	int cnt = 0;
	for (vector<HE_face *>::iterator p=faces.begin(); p!=faces.end(); p++) {
		HE_face *face = *p;
		HE_edge *fedge = face->tip;
		do {
			if (!vmark[fedge->vert]) {
			float curv = -2*M_PI;
			bool allsame = true;
			HE_edge *edge = fedge;

			do {
				if (edge->pair) {
					edge = edge->pair->prev;
				} else {
					allsame = false;
					vmark[fedge->vert] = true;
					break;
				}
			} while (edge!=fedge);
			if (!allsame) continue;
			edge = fedge;
			do {
				Vector3f v[3];
				for (int i=0; i<3; i++) {
					v[0].raw[i] = verts[edge->prev->vert*3+i];
					v[1].raw[i] = verts[edge->vert*3+i];
					v[2].raw[i] = verts[edge->next->vert*3+i];
				}
				//signed_angle = atan2(  N * ( V1 x V2 ), V1 * V2  );
				// where * is dot product and x is cross product
				// N is the normal to the polygon
				// ALL vectors: N, V1, V2 must be normalized
				Vector3f V1 = v[0]-v[1];
				Vector3f V2 = v[2]-v[1];
				Vector3f N  = V1^V2;
				N.normalize();
				V1.normalize();
				V2.normalize();
				float signed_angle = atan2(N * ( V1^V2 ), V1 * V2);
				curv += signed_angle;
				//			cerr << edge->vert << " " << signed_angle*180/M_PI << endl;
				edge = edge->pair->prev;
			} while (edge!=fedge);
			total_curv += fabs(curv);
			cnt++;
			vmark[fedge->vert] = true;
			}
			fedge = fedge->next;
		} while (fedge!=face->tip);
	}
//	cerr << "total curv: " << total_curv << " in " << cnt << " verts" << endl;
	return total_curv;
}

